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1. Introduction 


Position and attitude parameter estimation is often a prerequisite for controlling aerospace 
mechanical systems moving in space ( 1 ). Because projectile position often cannot be accurately 
estimated a priori due to various hardware and environmental uncertainties, guidance, navigation 
and control (GNC) systems are deployed onboard the object. 

Rocket-propelled missile GNC systems are quite mature ( 2 , 3), while effective GNC systems for 
accurate microunmanned vehicles are under development. For accurate, low-cost, and real-time 
applications, the design and implementation of the GNC systems are constrained by several 
technical challenges. Cost is by far the most challenging requirement, while computational 
efficiency is also required. In addition, robustness is needed in some cases for the desired GNC 
system to survive extremely harsh environments, such as gun-launched projectiles. Small, 
compact, and energy-efficient structures are also required for certain applications. 

The primary function of position estimation is to locate the coordinates of an object with respect 
to a set of reference points ( 4 ). In general, position estimation algorithms can be classified into 
three groups: methods based on time of arrival (TOA), time difference of arrival (TDOA), and 
angle of arrival (AOA). TOA methods capture the traveling time of signals between the object 
and the reference points, from which the distances between them are determined. Subsequently, 
the position of the object is estimated by the geometric relationship (5, 6 ). In order to measure 
the traveling time of signals, TOA systems need the accurate synchronization between the object 
and the reference points. TDOA methods determine the position of the object by examining the 
difference in time at which the signal arrives at multiple reference points ( 4 ). Thus TDOA 
systems require the accurate synchronization between the reference points. In addition, since the 
electromagnetic waves propagate at the constant speed of light, both TOA and TDOA systems 
demand a very high sampling rate to obtain high resolution of the time arrival. Similar to a 
global positioning system (GPS), the requirements of accurate synchronization and high 
sampling rate make TOA and TDOA systems expensive. AOA positioning, on the other hand, 
allows low-cost implementation. It determines the position of an object by the use of the 
estimated angles between the object and a set of reference points ( 7 - 9 ). A variety of algorithms 
have been proposed to estimate the angle of arrival ( 10 - 1 7). 

The primary function of the attitude estimation system is to determine the relative orientation 
between the coordinate frame fixed to the object and another reference coordinate frame ( 18 ). 
The attitude of the object can be described in three ways: direction cosine matrix (DCM), 
quaternion of rotation, and Euler angles ( 19 ). Wahba ( 20 ) first addressed the problem of 
determining the attitude of a rotating body by the vector-matching method. Subsequently, 
attitude estimation methods have relied mostly on the DCM ( 21 - 23 ). When the DCM 
description is used, the attitude parameters are linearly related to the observed vectors. Thus 
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linear estimation theory can be applied to the estimation of the DCM. DCM, however, uses nine 
parameters to describe the attitude, which increases computational complexity. The quaternion 
of rotation method applies Euler’s theorem, which states that any real rotation of one coordinate 
frame with respect to another may be described by a rotation through some angle about a single 
fixed axis ( 24 ). The quaternion method reduces the number of attitude parameters from nine to 
four. However, the quaternion parameters are nonlinearly related to the observed vectors. Some 
nonlinear approaches were proposed to determine the quaternion parameters, such as the Quest 
algorithm and quaternion Kalman filters ( 25 - 27 ). The third approach of attitude estimation uses 
Euler angles. The use of Euler angles is advantageous since the steering commands are explicit 
functions of the object’s Euler angles. In addition, the Euler angle method of attitude estimation 
requires determining only three parameters. Euler angles, however, are nonlinearly related to the 
observed vectors, and the attitude tracking using Euler angles is perhaps hampered by possible 
singularities ( 28 , 29 ). Recently, Wilson ( 30 ) developed a novel nonlinear approach to estimate 
the Euler angles of the projectile. However, this proposed system needs three expensive angular 
rate sensors. Subsequently, Wilson (37) extended this work and designed an attitude estimation 
system only based on a set of magnetometers. This approach, however, requires the exact 
knowledge of the object’s dynamic model with properly modeled forces and moments that 
sometimes are not observed. 

This report proposes a position and attitude navigation system, where both position and attitude 
can be iteratively estimated based on small and inexpensive sensor devices, such as uniform 
linear arrays (ULAs) and a magnetometer. It will be explained in section 2.4 that the position or 
attitude of the object cannot be estimated alone based on N beacons or a magnetometer—both 
are required. In addition, the algorithm for simultaneously estimating the position and attitude is 
not robust. Therefore, an iterative position and attitude estimation algorithm is developed in this 
report. In this iterative algorithm, the position is determined by AOA vector observations, and 
attitude is tracked using the Euler angle method. 

The system is accomplished in several steps. A set of ULAs on the object receiving the 
impinging RF signals transmitted from ground-based beacons is used to estimate the directions 
of arrival of the signals. In addition, since the signal-to-noise ratio (SNR) of the magnetometer is 
usually high, an aiding magnetometer is equipped on the spin axis of the object to improve the 
performance of the position and attitude estimation. The ULA and magnetometer measurements 
are exploited to estimate the position of the object based on the prior knowledge of the initial 
attitude parameters. Based on the position estimation results, the attitude parameters are 
estimated and updated. The resulting attitude parameter estimates are used to approximate the 
position parameters in the next estimation cycle. The equations in the algorithms have been 
developed assuming an ideal vertical magnetic field vector, therefore additional work in the form 
of coordinate transforms would be required for a generalized solution, which is not specifically 
addressed in this report. The proposed system is low-cost, and the estimates are simple and thus 
useful when GPS and expensive inertial measurement units (IMUs) are not available. 
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The report is organized as follows. The problem formulation is discussed in section 2. The 
position estimation based on three-dimensional (3-D) AOA location algorithm is developed in 
section 3. The attitude estimation algorithm based on two-dimensional (2-D) direction of arrival 
(DO A) estimation is developed in section 4. Simulations of the proposed algorithms are 
presented in section 5. Section 6 provides conclusions. 


2. Problem Formulation 


2.1 Coordinate Systems 

The motion of an object is usually described by rigid body equations of motion derived from 
Newton’s laws (29). This section summarizes and notates three kinds of coordinate systems. 

The first is the Earth-fixed coordinate system, which is fixed to the Earth with a fiat Earth 
assumption. Denote X, Y, and Z as the unit vectors pointing in the directions of the X, Y, and Z 
axes, respectively. Without loss of generality, the X, Y, and Z axes point to forward, right, and 
down, respectively. The second is the body-fixed coordinate system, with three unit vectors Xb, 
Y b , and Z b pointing to the X/„ Y/„ and Z* axes, respectively. The A, axis is along the object’s 
symmetric axis, referred to as the spin axis. The other two axes are perpendicular to the spin axis 
and each other. The Earth-fixed coordinate system and the body-fixed coordinate system are 
shown in figure 1. The third is the fixed-plane system, which is a body-fixed system that does 
not take the spin into account. The three axes are the X p , Y p , and Z p axes, where X p is overlapped 
with the Xb axis, and the Y p axis lies in the horizontal with respect to the Earth. 



Figure 1. Earth- and body-fixed coordinate systems and 
the Euler angle rotations. 
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The Earth- and body-fixed coordinate systems are related by a Euler rotation sequence. The 
Earth-fixed system is first rotated about the Z axis through the yaw angle y/, then about the new 
Y axis through the pitch angle 6, finally about the new X axis though the roll angle cp, which is 
shown in figure 1. From the Earth-fixed to body-fixed system, the transformation matrix is 

r \ 




C 0 

■V 

c e 

~ s e 

II 

<-C> 

c i// s e s f> 


S y S e S tj> 

+ c v c t 

C 6 S (p 

1 



s v s e c 4, 

~ c v s * 

C 0 C (p 


( 1 ) 


where s. is sin(») and c. is cos(»). If the roll angle is fixed as cp = 0, the transformation matrix 
from the Earth-fixed to fixed-plane system is 


T = 

ep 


c ¥ c e 

s c 

V « 

— s 

c 

V 

V 

C y/ S 0 



0 

C a 


( 2 ) 


The transformation matrix from the fixed-plane 


T = 

ep 


to body-fixed system is 
0 0 ^| 



( 3 ) 


2.2 Beacon Position and Attitude Navigation Aided by Magnetometer 

As shown in figure 2, suppose the position of an object is (xo,yo> z o) in 3-D free space and the 
attitude of the object is ( 6, if/, cp). A beacons are located on the ground at known positions 
(xj,y h Zi), i = l,...,N, serving as reference points for the position and attitude navigation. The 
beacons emit RF signals with their antenna patterns spanning 3-D free space. The N signals 
transmitted from the beacons are represented by the unit vectors of , s 2 s N , each working 
at frequencies f\,fi,...,f N , respectively. The unit vector is referred to as the ith AOA vector. 

As shown in figure 2, in the Earth-fixed coordinate, the elevation angles of the AOA vectors are 
a\, ci 2 ,..., a N , and the azimuth angles of the AOA vectors are b\, bo,..., b N . On the object, N sets of 
ULAs are equipped to receive impinging signals transmitted from the beacons. In order to 
distinguish the N signals, these ULAs are applied with different work frequencies of f\,fi,...,fa, 
corresponding to different signals. The structures of the equipped ULAs are shown in figure 3. 
As shown in figure 3, in the body-fixed coordinate, the elevation angles of the AOA vectors are 
a\, o.o,..., o.m, and the azimuth angles are p\, Pi,..., Pn ■ The directivities of the AOA vectors in the 
Earth-fixed coordinate are described by the elevation and azimuth angles {a h bi), while the 
directivities of the AOA vectors in the body-fixed coordinate are described by the elevation and 
azimuth angles (at,Pi), where i = 1, 2 ,...,N. In addition, a magnetometer is equipped on the spin 
axis of the object to measure the magnetic field vector in the body-fixed coordinate. The magnetic 
field vector in the Earth-fixed coordinate is along the Z-axis in figure 1. 
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Body-fixed 
coordination 



TZb 


N sets of uniform linear arrays (ULAs) with work 



A magnetometer 
is equipped along 
the spin axis 


Figure 3. The ULAs equipped on the object receive the impinging signals. 
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The voltage measurements from the N sets of ULAs are exploited by a 2-D DO A estimation 
algorithm, obtaining the estimation of the N pairs of elevation and azimuth angles in the 
body-fixed coordinate: {a l ,ft\ (d 2 ,ft 2 ),...,(a N ,ft N ). The ith pair of the elevation and azimuth 
angles (a i ,ft i ) is referred to as the DOA of the ith AOA vector in the body-fixed coordinate. 
Thus, the ULAs can only measure the AOA vectors in the body-fixed coordinate. In the 
body-fixed coordinate, 

ft = (sina. cos /? sin d i sin ft i cos ft ) T ,i = 1,2,..., N . (4) 


On the other hand, in the Earth-fixed coordinate, 

ft = (sin a i cos ft sin a i sin ft cos a t ) T , i = 1,2,..., N. 

A relationship between the AOA vectors in the body-fixed coordinate and in the Earth-fixed 
coordinate is described as follows. When the transformation relationship between the Earth- 
fixed and body-fixed coordinates is applied in equation 1, 


(5) 


ft ^ /V \ 

sin a i cos ft. 


c ¥ c e \ c e 

\ 

~ s e 

^sin a t cos ft'' 

sin a i sin ft 

= 

s ¥ s e s++c ¥ Ct 

c e s </> 

sin a. sin ft 

cos a i 


s v s e c^-c v s^ 

c e c t ; 

v cos a ( , 


for i =1, 2,...,7V. Thus, we have 


u = Hv, 


where 


u = [uf, vft ,..., f and • • • u. = (- sin ft cos ft , - sin d i sin ft , - cos ft ) r , 

v = [vf ,V 2 ,...,v^] r and---v i = (sina ; . cosb^sinar sin ft, cos aft 7 ,i = 1,2,..., A 


and 


H 


K eb 1 

0 


0 


eb2 


0 0 


0 ^ 
0 

r 

l ebN J 


A transformation of equation 6 is 

^sin a t cos b t ^ f 
sin a j sin b t 
y cos a t j 

where i = 1, 2,...,N. 


C y/ C e 


c v s o s * 


■w 


\ c e 

s ¥ s e St+c ¥ c+ 


\ C ¥ S 9 C <j, S y/ S $ 


S y/ S e C <f> 


c v s i 


c e s <t> 
c e C (j> J 


-V ■ 


V 


sin a i cos ft 

/V 

sin a i sin /?. 
cos a 


J 


( 6 ) 


(7) 

( 8 ) 
(9) 


( 10 ) 


( 11 ) 
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2.3 Magnetometer Model 

The magnetometer is a device that produces a voltage output proportional to the applied 
magnetic field in the direction of the magnetometer’s sensitive axis. The magnetometer is ideal 
for broad applications because it is very rugged and low cost. Let the sensitive axis be a unit 
vector i, and the local magnetic field vector is B. Then the measurement of an ideal 
magnetometer is the inner product of i and B 


M ideal = i B . (12) 

When the scale and shift influences of the analog circuitry are considered, the actual 
measurement of the magnetometer is 

M = s(i-B) + b, (13) 

where s is the scale factor and b is the bias offset. If the included angle between i and B is <9, 
equation 13 is rewritten as 


where B = |B|. Thus, 


M = sBcosO + b , 


<9 = cos 1 


f M-b^ 

v sB , 


(14) 

(15) 


In general, the SNR of the magnetometer is very high. However, some amount of noise is 
always present, which is modeled as white Gaussian noise (WGN). Therefore, the measurement 
of the magnetometer is adjusted as 

M = sBcosO + b + riM, (16) 

where «t/is WGN with zero mean and a variance of a m- 


2.4 Iterative Position and Attitude Estimation 

As described in section 2.2, the proposed navigation system measures one magnetic field vector 
and N AOA vectors. Based on the measurement vectors, it is proper to ask if these are sufficient 
to estimate either the position or the attitude. Consider the position estimation first. It will be 
proven in section 3.2 that three linearly independent vectors in the Earth-fixed coordinate with 
reference points are necessary and sufficient to determine the position of an object in 3-D free 
space. Since no reference point is associated to the magnetic field vector, it does not contribute 
to the estimation of position. Thus, the N AOA vectors must be used in the resection algorithm 
to estimate the position of the object (7). As mentioned in section 2.2, the ULAs equipped on the 
object can only measure the AOA vectors in the body-fixed coordinate. In order to obtain the 
AOA vectors in the Earth-fixed coordinate, the elevation and azimuth angles (a^b,) need to 
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be estimated from equation 11. In equation 11, the attitude parameters ( 9 , if/, <p) are needed to 
calculated (a, Aj- Thus, the position parameters cannot be estimated alone without attitude 
estimation. 

Next, consider the attitude estimation. It has been proven that two pairs of linearly independent 
vectors are necessary and sufficient to determine the attitude of an object (21). That means we 
have to know the two linearly independent vectors in both the Earth-fixed coordinate and the 
body-fixed coordinate. As described in section 2.2, the magnetic field vector is known in both 
coordinates, thus it provides one pair of vectors for the attitude estimation. In order to estimate 
the attitude parameters, we need to find another pair of linearly independent vectors from the N 
AOA vectors. Although the ULAs measure the AOA vectors in the body-fixed coordinate, we 
cannot obtain the AOA vectors in the Earth-fixed coordinate without the position of the object. 
Thus, we cannot find another pair of linearly independent vectors without position estimation. In 
an attitude estimation system, position information is usually obtained by GPS (18). Based on 
the previous analysis, the position or attitude cannot be estimated alone. 

Given that the position and attitude must be estimated jointly, the next question to address is if 
the position and attitude can be estimated simultaneously. The magnetometer measurement is 
shown in equation 16. The ULA measurements are described in equation 6, which can be 
rewritten as 


f A /V \ 

sin a t cos /?. 
sin d j sin /7 

— 

c ¥ c e 

\ c e 

\ 

s e 

c e s p 

((*, 

O'/ 

- x 0 )/norm i ' 
~ y a)/ norm i 

cos a, 

V ) 



s ¥ s e c t~ c v s t 

c e c <p; 

v( z « 

-z 0 )/norm iy 


2 2 2 1/2 

where norm; = {(x; - xo) + (>’, ~ yo) + (z, - zo) } • Thus, equations 16 and 17 consist of a 

T 

system of equations, where m = (xo,yo,zo, 9, '/A <p) are the unknown position and attitude 
parameters to be estimated. Equations 16 and 17 are nonlinear equations, where the unknown 
parameters are coupled together. In the simultaneous position and attitude estimation process, 
the parameter vector m must be estimated using recursive algorithms, such as a nonlinear least 
squares estimation algorithm. Our extensive simulations show that these simultaneous position 
and attitude estimation algorithms are not robust. If the SNR of the ULAs is smaller than 25 dB, 
the algorithm exploiting four beacons does not converge with very high probability. 

While the simultaneous position and attitude estimation is not robust, iterative position and 
attitude estimation algorithms are proposed next based on beacons and a magnetometer. 

Suppose the initial attitude of the object before launch is known as (9*,i//*,tp*). At the beginning 
of the estimation process, the initial attitude parameters of Up, if/, 9) are used to solve the 
elevation and azimuth angles (a,., 4) according to equation 9. Based on the estimation results of 
(a t , 4) , the position of (xo,yo^o) can be subsequently estimated based on the 3-D AOA location 
algorithm proposed in section 3.2. Based on equations 16 and 17 and the position estimation 
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results, the attitude parameters of (6, i//, <p) can be updated by the nonlinear estimation approach 
proposed in section 4.2. Then the current attitude estimation results will be exploited to update 
(a f ,4) i 11 the next iteration. The scheme of the iterative position and attitude estimation 
algorithms is illustrated in figure 4. The details of the position and attitude estimation algorithm 
are presented in sections 3 and 4. 



Figure 4. Iterative position and attitude estimation algorithm based on beacons 
and a magnetometer. 

In section 3.2, it is shown that three AOA vectors are necessary and sufficient to determine the 
position of the object. In the appendix, it is shown geometrically that two AOA vectors are 
necessary and sufficient to determine the attitude of the object. Therefore, in order to estimate 
both position and attitude, the proposed algorithms use three ground-based beacons. Since the 
magnetometer has very high SNR, an aiding magnetometer measures an additional magnetic 
field vector to improve the performance of the position and attitude estimation. 


3. Position Estimation Based on 3-D AOA Vectors 


3.1 2-D AOA Location Algorithm 

In this section, the 2-D AOA location algorithm will be first summarized and exploited as the 
foundation of the proposed 3-D AOA location algorithm. The 2-D AOA position algorithm, 
referred to as the resection algorithm, was proposed by Rappaport et al. ( 4 ) and McGillem and 
Rappaport (7). The coordinates for the analytic geometry solution of the AOA position location 
are shown in figure 5, where the position of points A, B, and C are (xi,vi), (X 2 ,>’ 2 ), and (x 3 ,y 3 ), 
respectively. The angle between AP and BP is y\. The angle between BP and CP is yi- The 
angle between AB and the horizontal direction is z\. The angle between BC and the horizontal 
direction is z^. Let the position of P be (xo,yo), which is calculated based on the following known 
parameters: 
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Figure 5. Coordinates for the analytic geometry solution of 
the 2-D AOA position location. 


and 


where 


2mx N + 2y N - 2mn 

y*= - 7—2 -^ 

1 + m 


x 0 = my 0 + n , 


m = jM y " , 

X N + X M 

R 2 2 -R { 2 -xl+xl-yl+yl 

n = - 

2(x n +x m ) 

x N = x, -R l sm(r l , 
y N =y 1 -R 1 cos(r l -y x ) , 
x M = x 2 -R 2 sin(r 2 -y 2 ) , 
y m =x 2 -R 2 cos(r 2 -y 2 ) > 

R - 

1 2 sin /j 


(18) 

(19) 


( 20 ) 

( 21 ) 

( 22 ) 

(23) 

(24) 

(25) 

(26) 
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and 


BC 


R 2 

2 sin y 2 

For the case of x 0 = my 0 + n , equations 18 and 19 maybe equivalently expressed as 

2m y N + 2x n + 2m n 


(27) 


x 0 =• 


l + m 


• —x. 


(28) 


and 

y 0 = m x 0 -n , 


(29) 


where m = — and n = — • 
m m 

3.2 Position Estimation Based on 3-D AOA Measurements 

In this section, a 3-D AOA location algorithm is generalized from the 2-D AOA location 
algorithm described in section 3.1. It is noted that the resection algorithm requires at least three 
AOA vectors to solve for the location in two dimensions. Suppose three beacons at known 
positions are available. It will be shown in the following section that the three beacons are 
sufficient to solve for the location in three dimensions, if the attitude of the object is assumed to 
be known as (0,y/,(j )). In order to estimate the position of the object in 3-D free space, the 3-D 
coordinates are first projected into the 2-D X-Y plane, as illustrated in figure 6 , where the three 
beacons are located at points A(xi,yi), B(x 2 ,yi), and Cfej'a), respectively. The position of the 
object in the X-Y plane is P(xo,yo)- The included angle between AP and BP is \bi - b\\, and the 
included angle between BP and CP is \b 2 - bi\, where b\, A, and 63 are the azimuth angles of 
, s 2 , and s 3 in the Earth-fixed coordinate, respectively. However, when the position of the 
object is unknown, b\, Z> 2 , and bj, cannot be determined. Therefore, b\, bi, and /13 need to be 
estimated from the attitude estimation results. According to equation 11, we have 
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Figure 6. The 3-D coordinates are projected into the 2-D 
X-Y plane. 


where i = 1, 2, 3. In equation 30, 6, \j/ , and (f) are the attitude estimation results, and a i and /?, 
are the estimation of elevation and azimuth angles of s i in the body-fixed coordinate, obtained 
from the fast 2-D DOA estimation algorithm described in section 4.1. According to equation 30, 
at and bi are estimated as follows: 

a i = cos -1 (s i3 ) 


and 


(31) 


tan -1 C?,. 2 /4) : 

b=[ tan -1 (4/4) + 2;r : 


> 0®—>0 
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■ < 0 ©—V>o 

sin a. sin a, 


(32) 


tan -1 (4/4) + ;r : 


otherwise 


These parameters are applied for the resection algorithm described in section 3.1 to estimate x 0 


and v 0 , where y l = 
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and t 2 = tan 
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In addition, the altitude of the object zo may be estimated from the geometric relationship 
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tan 2 g. = ( ^° ~ X,) 
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tan a. 


where i = 1, 2, 3. When the mean estimation is applied, z 0 can be estimated as 


1 3 

/V 1 /V 

Z —— 7 Z 
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Therefore, three beacons are necessary and sufficient to estimate the position of the object in 3-D 
free space. 


4. Attitude Estimation Algorithm Based on 2-D DOA Estimation 


4.1 A Fast 2-D DOA Estimation Algorithm 

Several 2-D DOA estimation algorithms have been presented in the literature. Maximum 
likelihood methods with high computational complexity were first proposed (10,11). MUSIC 
and ESPRIT algorithms exploited either eigen value decomposition or singular value 
decomposition to obtain high resolution in DOA estimation (12-15). Wu et al. (16) proposed a 
2-D DOA estimation algorithm based on propagator method. Recently, Tayem and Kwon (17) 
proposed a computationally efficient azimuth and elevation angle estimation algorithm with no 
failure and no eigen decomposition. The computational efficiency and accuracy of this 
algorithm are suitable for the real-time estimation of a rapidly moving object. This fast 2-D 
DOA estimation algorithm is summarized in the appendix. It will be exploited to formulate the 
position and attitude estimation algorithms. 

Figure 7 shows the proposed array configuration consisting of three ULAs with interspacing d 
equal to a half wavelength of incident signals. The three ULAs consist of N, N+l, and N 
elements, respectively. The first and third ULAs are referred to as subarrays Z and W, 
respectively. The first and last N elements in the second ULA are referred to as subarrays X and 
Y, respectively. Thus, subarrays X, Y, Z, and W consist of N elements. Suppose there are K 

T 

narrow-band sources, s (t) = (s\(t),S 2 (t),...,SK (f)] , with the same wavelength X impinging on the 
arrays. The received signals at the subarrays X, Y, Z, and W are 
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-Subarray Z-► 



-Subarray W- ► 



Figure 7. The proposed array configuration equipped on the 
object. 


x(0= As(0 + n x (t), 
y(0= AO y s(t) + n v (r), 
z(t)= AO z s(t) + n z (t) , 
w(0= A<D w s(t) + n H (f), 

t=l,2,...,L, (36) 

where 


A - [a(ai, y?i), a(cir 2 , /%), .. &(cck, Pk) ], 


(37) 
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. 2tzc/ sin a*. sin/?*. 


/I 


0 Z =diag[c/ ] ,< 7 2 ,...,g A ], where q K = exp 


27udcosa, \ 


-]■ 


A 


=diag[r 1 ,r 2 ,...,r jC ], where r K =exp -j 


,2ml sin a k cos (3 k 


A 


(38) 

(39) 

(40) 

(41) 


where k = 1, 2,...,K. Let g(t) = [x r (t), y r (t), z T (t), w r (t)] 7 denote a 4A x 1 snapshot data vector 
with t= 1, 2,...,L. Let F = [g(l), g(2),..., g(L)] denote a 4/V x L data matrix consisting of L 
snapshots. The partition of F is F = [F/, F 2 7 '] 7 ', where Fi and F 2 are submatrices with 
dimensions of K* L and (4 N—K) x L, respectively. Let P = (F|F 1 /7 ) _ 1 F 1 ¥ 2 'denote the K x 
(AN— K) propagator estimate matrix based on the data matrix F. The partitions of P are 
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P H ’ H = [P, 7 , P 7 , Pf, P l P 5 r , Pf, P 7 r f, where the dimension of P, 7 , P[, Pi and P 7 r is (N- K) x K, 
and the dimension of P 2 r , P[, and P 6 r is K x K. Thus, the diagonal elements of O l7 d> z , and <D W . 
respectively, can be estimated by finding the K eigen values of P 3 P*, P 5 P* and P 7 P*, where # 

denotes the pseudoinverse. Finally, the azimuth and elevation angles for each source can be 
estimated as 


P k =tan" 


arg(O v ) 


yJkk 




k= 1 , 2 , ...,K. 


(42) 


a k = tan" 


arg(O u ) 


kk 


arg(O z ) tt cos/?, 


k = 1 , 2 , 


(43) 


4.2 Attitude Estimation Algorithm Based on Fast 2-D DO A Estimation 

In this section, an attitude estimation algorithm is proposed based on the fast 2-D DOA 
estimation described in section 4.1. Suppose one magnetometer is equipped on the spin axis of 
the object. According to equation 15, and assuming b = 0 and sB = 1, the pitch angle (assuming 
a vertical magnetic field vector) is estimated as 

# = -sin _ 1 (M) , (44) 

where M is the measurement of the magnetometer. 

On the other hand, it has been proven that two pairs of linearly independent vectors are necessary 
and sufficient to determine the attitude of the object (27). That means that given the position of 
the object, two ground-based beacons are necessary and sufficient to determine the attitude. The 
appendix gives a geometric explanation for this. Our proposed navigation system includes three 
beacons that are redundant to estimate the attitude. Corresponding to the three beacons, three 
sets of ULAs are equipped on the object to receive impinging signals. As described in section 
3.2, the current position parameters have already been estimated as (x 0 ,y 0 ,z 0 ). Let the positions 

of the three beacons be (xi,yi,zi), (x 2 ,y 2 ,zi), and (* 3 , 73 ,Z 3 ), respectively. Based on equation 17, we 
have 


sin a. cos p. 

sin a. sin /?. 
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cos a. J 



S v S e C * ~ C y S 4> y 

v ( z ,--a), 
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1 1 J 


where norm, = {(x, -x 0 ) 2 + (y j -y 0 ) 2 + (z ; -z 0 ) 2 } 1/2 and i = 1, 2, 3. Thus, we have 

u = Hv , (46) 
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where 


u = [u[,U 2 ,...,u^,] r and---u i =(-sina ; .cos/?., - sina ; .sin/?.,-cos 4 ) r , (47) 

v = [v^V 2 ,...,v£fand"-v i = (sina ; . cosZ^sina,. sinb^cosaX ,i = 1,2,..., N , (48) 


and 
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Let s n = J—4 , 4 = J—4 , and 4 = J—4 . From the first equation of equation 45, we 
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have 


cos (y/.-y.) = 
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(50) 


where y can be estimated as 


tan -1 (i ;2 /4) 
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(51) 


tan” ( 4 / 4 ) +^ : 


otherwise 


Since the range of y/ i - y. is unknown, y i cannot be uniquely estimated from equation 50. The 
estimation ij/ i from the last iteration then provides an estimate of the range of y/ i - y t . Let 4 be 
the estimation result of \|/ ( in the iteration. Then in the k+ 1 th iteration, 

y ( . - 271 + A 
y.-A 

Yi + A 
y ; . + 27i-A 

where 



Yi ~ i + [-2ti,-tt] 
Yi-li e[-7t,0] 
4-y, e[0,7i] 
4-y, e[7i,27i] 


(52) 
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A = cos 


-i 


^sin a i cos ft. + s~s i3 ^ 


Averaging over the three sets of estimations, we have 

v = - 2 _ t Y i . 

-> 1=1 

From the second and third equations of equation 45, we have 

sin a i cos /?. = m x sin (j) + m 2 cos </> 
cos a i = m 3 sin (j) + m 4 cos <j> 

where 


m , = sin a,, cos b,c-s- + sin a,, sin bjS-s- + cosa,c = 

1 l 1 Y U 1 1 r U 1 U 
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m 2 - sina ( . sinZrc^ -sina ; cos 
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Thus, for each i, sin (f> : and cis(f) i can be solved as 
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Therefore, 


-1 ^ ^ 
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= ( tan" 1 (sin ^ /cos ^) + 2n 
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sin^. < 0® cos$ <0 
otherwise 


Applying an averaging operation over the three sets of estimations, we have 

1 3 

/v I —-, /v 

3 i =1 


(53) 


(54) 


(55) 


(56) 


(57) 
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(59) 


(60) 
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5. Simulations 


5.1 Performance-Enhancement Techniques 

In section 4.2, equations 54 and 60 are intended to give good estimations of the yaw and roll 
angles, respectively. However, when the denominators in equations 50 and 57-59 are small, the 
errors of the estimations are magnified. In addition, in the iterative position and attitude 
estimation algorithms, the error may be transferred between each other and accumulated. In 
order to further improve the estimation performance, a recursive median filter is applied in the 
estimation process to remove large deviations from the estimation results (32). The output of the 
recursive median filter with window width equal to 2N + 1 is 

x M (t) = median [x M (t - NAt),...,x M (t - At),x(t),x(t+At),...,x M (t+NAt)] ’ ^1) 

where x is the arbitrary estimated parameter and At is the sampling interval. In the recursive 
median filter, the prior outputs are used in the current filter step. As shown in equation 61, 
x M (t - NAt),..., x M (t - At) are prior outputs. Since the SNR of the magnetometer is usually very 

high, the recursive median filter is not applied to the estimation of the pitch angle. 

5.2 Simulations of Iterative Position and Attitude Estimation Algorithms 

The proposed iterative position and attitude estimation algorithms are simulated using a 
gun-launched object. In these simulations, the object is launched with an initial pitch angle 
6*o = 45°, initial yaw angle i//o = 0°, initial roll angle tpo = 0°, and initial position at (0, 0, 0). The 
trajectory of the object is a standard parabola. The range of the trajectory is 3 km and the 
traveling time is 2 s. Three ground-based beacons are located at (-1500, 3000, 0), (4000, 1000, 
0), and (6000, -4000, 0), respectively. The work frequencies of the three beacons are 
/i = 10 GHz ,f 2 = 5 GHz, and = 7.5 GHz, respectively. All the subarrays of the ULAs equipped 
on the object consist of N= 4 elements. Assume the noise on the ULAs is WGN, with SNR = 20 
dB. The noise on the magnetometer is also WGN, with the variance o~ M = 10 6 . 

The relative locations between the trajectory of the object and the three beacons are illustrated in 
figure 8. Recursive median filters with window width equal to 7 are applied to the estimations of 
y/, cp, xo,yo, and zq. The simulation results are shown in figures 9-14, corresponding to the 
estimations of coordinates on the X-axis, Y-axis, Z-axis, and pitch angle, yaw angle and roll 
angle, respectively. 
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Figure 8. Relative locations between the trajectory of the object and the 
three beacons. 



Figure 9. Simulation result for the estimation of x 0 . SNR = 20 dB. 
MSE = 2.63 x 10 4 . 



Figure 10. Simulation result for the estimation of y 0 . SNR = 20 dB. 
MSE = 1.11 x io 4 . 
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Figure 11. Simulation result for the estimation of z 0 . SNR = 20 dB. 
MSE = 2.37 x 10 5 . 



Figure 12. Simulation result for the estimation of pitch angle. SNR 
= 20dB. MSE= 1.31 x 10' 6 . 
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Figure 13. Simulation result for the estimation of yaw angle. 
SNR = 20 dB. MSE = 3.37 x 10' 4 . 
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Figure 14. Simulation result for the estimation of roll angle. 

SNR = 20dB. MSE = 2.97 x 10" 1 . 

In figures 9-14, the MSE of x 0 is 2.63 x 10 4 , the MSE of y 0 is 1.11 x 10 4 , the MSE of z 0 is 
2.37 x 10 5 , the MSE of 6 is 1.31 x 10“ 6 , the MSE of y> is 3.37 x 10 4 , and the MSE of $ is 
2.97 x Iff 1 . The recursive median filters successfully remove some large deviations from the 
estimation results. However, it is noted that there are still some singular points in figures 9-11, 
13, and 14, which result in the large MSEs of Jc 0 , y 0 , and z 0 . There are two reasons for the 
singularity. First, the denominators in equations 57-59 are close to zero. Second, the use of ft 
to guess the range of (j) l + - y, in equation 52 can sometimes be inaccurate. In addition, the 
period of these deviations is longer than the window period of the used recursive median filters. 
Therefore, the recursive median filters cannot remove these deviations. In our future research, 
some approaches will be proposed to eliminate the singular points of the iterative position and 
attitude estimation algorithms, which are out of the scope of this report. 


6. Conclusion 


This report proposes robust low-cost and real-time position and attitude estimation algorithms 
based on ground-based beacons and magnetometers. Three sets of ULAs equipped on the object 
with distinguishable work frequencies receive signals transmitted from three ground-based 
beacons. The directions of the AOA vectors are estimated by fast 2-D DOA estimation 
algorithm. Based on the prior knowledge of initial attitude parameters, the position of the object 
is estimated based on a 3-D AOA location algorithm. Based on the position estimation results, 
current attitude parameters are updated and used to estimate the position parameters in the next 
estimation cycle. A recursive median filter is applied to remove large deviations from the 
estimation results and improve the performance. Simulations show that the proposed low-cost 
and real-time algorithms successfully estimate the position and attitude most of the time. The 
algorithms developed for this technique are applicable to the special case of a perfectly vertical 
magnetic field, thus additional angular transforms would need to be introduced for solutions to 
more general magnetic orientation cases. 
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Appendix. Fast Two-Dimensional Direction of Arrival Estimation Algorithm 


Consider one impinging signal represented by the vector s that is received by a set of parallel 
uniform linear arrays (ULAs) equipped on the object, as shown in figure A-l, and assume that 
the position of the object is known. 


Vb 

> 


Body-fixed 

coordination 


T Z b 


Three parallel uniform 
linear arrays (ULAs) 


0 : • 


4 P 


Figure A-l. One impinging signal is received by the 

system of three parallel ULAs equipped on the 
object. 


When the fast two-dimensional direction of arrival estimation algorithm is used, the elevation 
angle a and the azimuth angle /? are estimated as a and /?, respectively. According to the 
cosine theorem, the included angle between the Xf, axis and ? is 

co x = cos -1 (cos/?cos(90°- a)) = cos -1 (cos/?sin a). (A-l) 

The direction of s in the Earth-fixed coordinate can be determined by the positions of the 
beacon and the object. Based on the signal direction s and the included angle co , a cone with an 
axis of ? can be determined, on whose surface infinite, body-fixed coordinates can be found to 
satisfy the estimated elevation and azimuth angles. Figure A-2 shows two sets of body-fixed 
coordinates ( X b , Y b , Z b ) and ( X h , Y h , Z h ) satisfying the given conditions. Therefore, given 

the position of the object, one angle of arrival (AOA) vector is not sufficient to determine the 
attitude of the object. 
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Figure A-2. One impinging signal determines a cone, 
on whose surface infinite body-fixed 
coordinates can be found to satisfy the 
estimated elevation and azimuth angles. 


If two impinging signals are transmitted with direction vectors of s 1 and s 2 , two cones with axes 
of and s 2 are determined by the included angle between the Xb axis and the vector directions. 
Let the centers of the undersurfaces of the two cones be 0\ and Of- There are at most two 
intersection lines between the two cones, OA and OB , as shown in figure A-3. Let the 
estimated elevation and azimuth angles from the first impinging signal be ( a x , ) and the 

estimated elevation and azimuth angles from the second impinging signal be ( a 2 , /? 2 ). In figure 
A-3, assume that the body-fixed coordinate ( X h , Y b , Z h ) on the intersection line OA satisfies 
the elevation and azimuth angles ( a x , /?,) and ( a 2 , fi 2 ) with respect to and s 2 , respectively. 
On the intersection line OB , body-fixed coordinate ( X ' b , Y h , Z h ) satisfies the elevation and 
azimuth angles (a t , /?,) with respect to . Body-fixed coordinate ( X " h , Y h , Z" h ) satisfies the 
elevation and azimuth angles ( a 2 , [i 2 ) with respect to s 2 , where X b overlaps X b . Thus, only ( 
X b , Y b ,Z b ) satisfies all of the given conditions. In conclusion, given the position of the object, 
two linearly independent AO A vectors are sufficient to determine the attitude of the object. 
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Figure A-3. Two impinging signals determine a pair of 
attitudes of the object. 
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RDRL D 

2800 POWDER MILL RD 
ADELPHI MD 20783-1197 


ABERDEEN PROVING GROUND 

1 DIR USARL 

RDRL CIM G (BLDG 4600) 
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NO. OF 

COPIES ORGANIZATION 


NO. OF 

COPIES ORGANIZATION 


1 CDR US ARMY TACOM ARDEC 

AMSRD AAR AEP 
M CILLI 
BLDG 382 

PICATINNY ARSENAL NJ 07806-5000 

1 US ARMY TARDEC 
AMSRD TAR R 

J WHITE 
MS 263 

WARREN MI 48397-5000 

2 COMMANDER 

US ARMY TACOM ARDEC 
AMSRD AAR AEP E 
D CARLUCCI 
M HOLLIS 
BLDG 94 

PICATINNY ARSENAL NJ 07806-5000 

1 COMMANDER 

US ARMY TACOM ARDEC 
AMSRD AR AEP S 
RWERKO 
BLDG 94 

PICATINNY ARSENAL NJ 07806-5000 

1 COMMANDER 

US ARMY TACOM ARDEC 
AMSRD AAR AEM C 
P MAGNOTTI 
BLDG 61S 

PICATINNY ARSENAL NJ 07806-5000 


RDRL WML D 
R BEYER 
M NUSCA 
P CONROY 
RDRL WML E 
B GUIDOS 
P WEINACHT 
F FRESCONI 
J GARNER 
JSAHU 
RDRL WML F 
E BUKOWSKI 
B DAVIS 
K HUBBARD 
T HARKINS 
D HEPNER 
MILG 

G KATULKA (5 CPS) 
D LYON 
P MULLER 
B TOPPER 
P PEREGINO 
RHALL 

D GRZYBOWSKI 
D PETRICK 
RDRL WML G 
T BROWN 
RDRL WML H 
B SORENSEN 
C CANDLAND 


1 PM CAS 

SFAE AMO CAS 
PMANZ 
BLDG 172 

PICATINNY ARSENAL NJ 07806-5000 


ABERDEEN PROVING GROUND 


37 DIRUSARL 
RDRL WM 
J SMITH 
P PLOSTINS 
RDRL WML 
M ZOLTOSKI 
JNEWILL 
T VONG 
RDRL WML A 
B OBERLE 
R PEARSON 
A THOMPSON 
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